#include "swarmSocketServer.h"
#include "../ControlLib/LED.h"
#include "../ControlLib/Motor.h"
#include <vector>
#include <string>
#include <cstdlib>
#include <iostream>
#include "../ControlLib/send.h"
using namespace std;

int main()
{
	//LED rd(RED), be(BLUE), gn(GREEN);
	SPIInit();
	swarmSocketServer server(5555, false);
	vector<vector<string> > ms;
	LED rd(RED), be(BLUE), gn(GREEN);
	Motor mr;
	int com;
	char c;
	cout<< "hello\n";
	while(1)
	{
		ms = server.read();
		for(int i = 0; i < ms.size(); i++)
		{
			com = atoi(ms[i][0].substr(1, ms[i][0].size() - 1).c_str());
			c = ms[i][0][0];
			cout<< ms[i][0] << " com " << com << " from " << ms[i][1] << endl;
			//send((unsigned char) com);
			if(c == '<')
			{
				mr.leftWheelVar((char) com);
			}
			else if(c == '>')
			{
				mr.rightWheelVar((char) com);
			}
			else {
				switch(com)
				{
					case 16:
						rd.setLED(false);
						break;
					case 17:
						rd.setLED(true);
						break;
					case 32:
						gn.setLED(false);
						break;
					case 33:
						gn.setLED(true);
						break;
					case 48:
						be.setLED(false);
						break;
					case 49:
						be.setLED(true);
						break;
					case 64:
						mr.rightWheelStop();
						break;
					case 65:
						mr.rightWheelGo();
						break;
					case 128:
						mr.leftWheelStop();
						break;
					case 129:
						mr.leftWheelGo();
						break;
					case 192:
						mr.stop();
						break;
					case 193:
						mr.go();
						break;
					case 197:
						mr.spinLeft();
						break;
					case 198:
						mr.spinRight();
						break;
					default:
						cout<< "no good\n";
					break;
				}
			}
		}
	}
	return 0;
}

